/*
 * Copyright 2009, 2010 IPOL Image Processing On Line http://www.ipol.im/
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * \file ami_pol.c
 * \brief library to estimate real polynomial roots.
 * \author Luis Alvarez
*/

#include "ami_pol.h"
#include "stdlib.h"
#include "stdio.h"


/**
 * \fn long double ami_horner(long double *pol,int degree,long double x,long double *fp)
 * \brief function to evaluate a polynomial and its derivate using Horner Algorithm
 * \return polynomial evaluation
 * \author Luis Alvarez
*/
long double ami_horner(
long double *pol /** coefficient polynomial vector pol[0]+pol[1]*x+pol[2]*x^2+... */,
int degree /** polynomial degree */,
long double x /** point to evaluate the polynomial */,
long double *fp /** output polynomial derivate evaluation */)
{
  long double PPX=pol[degree],PX=pol[degree];
	int k;
	for(k=degree-1;k>0;k--){
	  PX=PX*x+pol[k];
	  PPX=PPX*x+PX;
	}
	PX=PX*x+pol[0];
	*fp=PPX;
	return(PX);
}

/**
 * \fn long double ami_root_bisection(long double *pol,int degree,long double a,long double b,long double TOL)
 * \brief function to compute the polynomial root in an interval. We use a combination of bisection and Newton-Raphson tecniques
 * \return polynomial root in the interval
 * \author Luis Alvarez
*/
long double ami_root_bisection(
long double *pol /** coefficient polynomial vector pol[0]+pol[1]*x+pol[2]*x^2+... */ ,
int degree  /** polynomial degree */,
long double a  /** left interval extreme */,
long double b /** right interval extreme */,
long double TOL /** convergence accuracy*/)
{
  long double a2=a,b2=b,fa,fb,c,c0,fc,fp;
  
  fp=0.0; /*Added by Luis Gomez*/

  /* we evaluate the polynomial at interval extrema */
  fa=ami_horner(pol,degree,a,&fp);
  fb=ami_horner(pol,degree,b,&fp);
  /* we check if there is a root in the interval */
  if(fa*fb>0) return(a);
  /* we evaluate the interval middle point */
  c=(a2+b2)*0.5;
  c0=c+1e30;
  /* we perform Newton-Raphson or Bisection iterations */
  while( (b2-a2)>(TOL*fabs(b2)+1e-10) &&  fabs(c-c0)>(TOL*fabs(c0)+1e-10) ){
    fc=ami_horner(pol,degree,c,&fp);
    /* we try Newton-Raphson */
    c0=c;
    if(fp!=0.){
      c=c-fc/fp;
      /* printf("c=%f\n",(double) c); */
      if( c>=a2 && c<=b2 ) continue;
    }
    /* if Newton-Raphson fails, we try bisection */
    c=(a2+b2)*0.5;
    c0=c+1e30;
    /* we check if we have arrive to the root */
    if(fabs(fc)<TOL) break;
    /* we perform bisection iteration */
    if((fa*fc)<0){
       b2=c;
       fb=fc;
    }
    else{
       a2=c;
       fa=fc;
    }
    c=(a2+b2)*0.5;
  }
  return(c);
}

/**
 * \fn int ami_polynomial_root(double *pol, int degree, double *root_r, double *root_i)
 * \brief function to estimate real polynomial roots. We use a recursive procedure computing the derivative polynomial roots.
          This function only works for relative low polynomial degree
 * \return The number of real polynomial root
 * \author Luis Alvarez
*/
int ami_polynomial_root(
double *pol /** coefficient polynomial vector pol[degree]+pol[degree-1]*x+pol[degree-2]*x^2+... FOR HISTORICAL REASONS THE
                ORDER POLYNOMIAL COEFICIENT IS GIVEN IS DIFFERENT THAT IN THE OTHERS POLYNOMIAL FUNCTIONS */,
int degree /** polynomial degree */,
double *root_r /** output real component of polynomial roots */,
double *root_i /** output complex component of polynomial roots. Since this function only real roots this complex part is fitted to 0.
                 FOR HISTORICAL REASONS WE KEEP THIS COMPLEX PART IN THE FUNCTION*/)
{

  long double a,b,*p,*p_aux,*ap,*f,fp,*pol2,fa,fb,TOL=1e-14;
  long double max,n_factor;
  int m,k,l,Nr,N=degree;

  p=p_aux=ap=f=pol2=NULL;
  fp=0.0;a=b=fa=fb=max=n_factor=0.0; /*Added by Luis Gomez*/
  Nr=0;/*Added by Luis Gomez*/
 
  /* WE ALLOCATE MEMORY */
  p=(long double*)malloc(sizeof(long double)*(N+2));
  p_aux=(long double*)malloc(sizeof(long double)*(N+2));
  ap=(long double*)malloc(sizeof(long double)*(N+1));
  f=(long double*)malloc(sizeof(long double)*(N+1));
  pol2=(long double*)malloc(sizeof(long double)*(N+2));

  /* POLYNOMIAL NORMALIZATION */
  if(pol[0]!=1.){
    for(k=0;k<=N;k++) pol[k]/=pol[0];
  }

  /* WE REORDER THE POLYNOM */
  for(k=0;k<=N;k++) pol2[k]=pol[N-k];
  pol2[N+1]=0.0; /*Added by Luis Gomez*/

  /* WE NORMALIZE POLINOMIAL COEFFICIENTS TO AVOID POLYNOMIAL EVALUATION IN LARGE NUMBERS*/
  n_factor=0;
  for(k=0;k<N;k++){
    if(fabs(pol2[k])>10.){
      max=log(fabs((double) pol2[k])/10.)/(N-k);
      if(max>n_factor) n_factor=max;
    }
  }

  n_factor=exp((double) n_factor);
  max=n_factor;
  for(k=N-1;k>=0;k--){
    pol2[k]/=max;
    max*=n_factor;
  }

  /* WE COMPUTE FACTORIAL NUMBERS */
  f[0]=1.;
  for(k=1;k<=N;k++) f[k]=f[k-1]*k;

  /* WE COMPUTE THE INITIAL INTERVAL WHERE ALL ROOTS ARE INCLUDED */
  max=fabs(pol2[0]);
  for(k=1;k<N;k++){
    if(fabs(pol2[k])>max)
      max=fabs(pol2[k]);
  }
  
  /* WE INITIALIZE THE INTERVALS */
  p[0]=-1.-max/fabs(pol2[N]);
  /* WE COMPUTE THE POLYNOMIAL DEGREE-1 DERIVATE ROOT */
  p[1]=-(pol2[N-1]*f[N-1])/(pol2[N]*f[N]);
  for(k=2;k<=(N+1);k++) p[k]=-p[0];
  for(k=0;k<=N;k++)p_aux[k]=p[k]; 
  p_aux[N+1]=0.0; /*Added by Luis Gomez*/
  /* WE COMPUTE THE DERIVATIVE POLINOMIAL ROOTS IN A RECURSIVE WAY */
	for(k=N-2;k>=0;k--){
	  /* we compute polynomial derivative coefficients */
		for(l=0;l<=(N-k);l++){ ap[l]=pol2[l+k]*(f[k+l]/f[l]); }
		/* we check the valid intervals to compute derivative polynomial roots */
		m=1;
		fa=ami_horner(ap,N-k,p_aux[0],&fp);
		for(l=1;l<=(N-k);l++){
     		 fb=ami_horner(ap,N-k,p_aux[l],&fp);
      		 if((fa*fb)<=0 || fabs(fb)<=TOL){
       			p[m++]=p_aux[l];
        		fa=fb;
      		 }
		}
		for(l=m;l<N;l++) p[l]=p[N];
		/* we compute the derivative polynomial roots in each interval */
		for(l=1;l<=(N-k);l++){  p_aux[l]=ami_root_bisection(ap,N-k,p[l-1],p[l],TOL); }
			}
	/* WE STORE THE ROOTS */
	root_i[0]=0.; /* we fit the complex component of the root to 0 */
	Nr=0;
	for(k=1;k<=N;k++){
	  /* we check if the polynomial has a root in the point */
	  a=(p_aux[k]+p_aux[k-1])*0.5;
	  b=(p_aux[k]+p_aux[k+1])*0.5;
	  fa=ami_horner(pol2,degree,a,&fp);
	  fb=ami_horner(pol2,degree,b,&fp);
	  if(fa*fb<0 || fabs(ami_horner(pol2,degree,p_aux[k],&fp))<TOL){
           root_i[Nr]=0; /* we fit the complex component of the root to 0 */
	   root_r[Nr++]=p_aux[k]*n_factor; /* we denormalize the root */
	  }
	}
  /* we free the memory */
  free(p); free(ap); free(f); free(pol2); free(p_aux);
  /* we return the number of real roots estimated */
  return(Nr);

}

